www.gusucode.com > ROS Deployed on Arduino running in External Mode in Simulink 程序工具箱matlab源码 > ROS Deployed on Arduino running in External Mode in Simulink/sfun_ros_interface.cpp
/* * sfun_ros_interface.cpp: S-Function for Demonstrating ROS Deployed to * Arduino from within Simulink * * Using Template from sfuntmpl_basic from Mathworks and linked with code * from rosserial_arduino * * Author: Gavin Kane * gavin.kane@hs-emden-leer.de * * License: BSD * * * Prepare for the deployment to arduino by calling * "mex sfun_ros_interface.cpp" * in the matlab command window */ #define S_FUNCTION_NAME sfun_ros_interface #define S_FUNCTION_LEVEL 2 /* * Need to include simstruc.h for the definition of the SimStruct and * its associated macro definitions. */ #include "simstruc.h" /* * ROS and ARDUINO Defines and Includes */ uint16_T servoValue1 = 0; boolean_T outputEnable = false; #define USE_STM32_HW_SERIAL #define ARDUINO 100 /* * All ROS and Arduino Code must be kept inside the #ifndef lines */ #ifndef MATLAB_MEX_FILE #include <ros.h> //#include <std_msgs/String.h> #include <std_msgs/Empty.h> #include <std_msgs/UInt16.h> #include <rosserial_arduino/Adc.h> ros::NodeHandle nh; void servo_cb_1( const std_msgs::UInt16& cmd_msg){ servoValue1 = cmd_msg.data; } void messageCb( const std_msgs::Empty& toggle_msg){ digitalWrite(13, HIGH-digitalRead(13)); // blink the led outputEnable = !outputEnable; } ros::Subscriber<std_msgs::Empty> sub_led("toggle_led", messageCb ); ros::Subscriber<std_msgs::UInt16> sub_link_1("robot_link_1", servo_cb_1 ); rosserial_arduino::Adc adc_msg; ros::Publisher adc_pub("adc", &adc_msg); #endif /*====================* * S-function methods * *====================*/ /* Function: mdlInitializeSizes =============================================== * Abstract: * The sizes information is used by Simulink to determine the S-function * block's characteristics (number of inputs, outputs, states, etc.). */ static void mdlInitializeSizes(SimStruct *S) { ssSetNumSFcnParams(S, 0); /* Number of expected parameters */ if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { /* Return if number of expected != number of actual parameters */ return; } ssSetNumContStates(S, 0); ssSetNumDiscStates(S, 0); if (!ssSetNumInputPorts(S, 1)) return; ssSetInputPortWidth(S, 0, 6); ssSetInputPortDataType(S, 0, SS_UINT16); ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/ /* * Set direct feedthrough flag (1=yes, 0=no). * A port has direct feedthrough if the input is used in either * the mdlOutputs or mdlGetTimeOfNextVarHit functions. */ if (!ssSetNumOutputPorts(S, 2)) return; ssSetOutputPortWidth(S, 0, 1); ssSetOutputPortDataType(S, 0, SS_UINT16); ssSetOutputPortWidth(S, 1, 1); ssSetOutputPortDataType(S, 1, SS_BOOLEAN); ssSetNumSampleTimes(S, 1); ssSetNumRWork(S, 0); ssSetNumIWork(S, 0); ssSetNumPWork(S, 0); ssSetNumModes(S, 0); ssSetNumNonsampledZCs(S, 0); /* Specify the sim state compliance to be same as a built-in block */ ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE); ssSetOptions(S, 0); } /* Function: mdlInitializeSampleTimes ========================================= * Abstract: * This function is used to specify the sample time(s) for your * S-function. You must register the same number of sample times as * specified in ssSetNumSampleTimes. */ static void mdlInitializeSampleTimes(SimStruct *S) { ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME); ssSetOffsetTime(S, 0, 0.0); } #define MDL_START /* Change to #undef to remove function */ #if defined(MDL_START) /* Function: mdlStart ======================================================= * Abstract: * This function is called once at start of model execution. * * In this section we start up the node and advertise and subscribe to * topics as applicable */ static void mdlStart(SimStruct *S) { #ifndef MATLAB_MEX_FILE pinMode(13, OUTPUT); nh.initNode(); nh.advertise(adc_pub); nh.subscribe(sub_led); nh.subscribe(sub_link_1); #endif } #endif /* MDL_START */ /* Function: mdlOutputs ======================================================= * Abstract: * In this function, inputs are used for the data to be published, and * data gained from previous callbacks are set to the outputs. */ static void mdlOutputs(SimStruct *S, int_T tid) { const uint16_T *u = (const uint16_T*) ssGetInputPortSignal(S,0); uint16_T *servo = (uint16_T*)ssGetOutputPortSignal(S,0); boolean_T *led_enable = (boolean_T*)ssGetOutputPortSignal(S,1); #ifndef MATLAB_MEX_FILE adc_msg.adc0 = u[0]; adc_msg.adc1 = u[1]; adc_msg.adc2 = u[2]; adc_msg.adc3 = u[3]; adc_msg.adc4 = u[4]; adc_msg.adc5 = u[5]; adc_pub.publish(&adc_msg); nh.spinOnce(); #endif servo[0] = servoValue1; led_enable[0] = outputEnable; } /* Function: mdlTerminate ===================================================== * Abstract: * In this function, you should perform any actions that are necessary * at the termination of a simulation. For example, if memory was * allocated in mdlStart, this is the place to free it. */ static void mdlTerminate(SimStruct *S) { } /*=============================* * Required S-function trailer * *=============================*/ #ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ #include "simulink.c" /* MEX-file interface mechanism */ #else #include "cg_sfun.h" /* Code generation registration function */ #endif